/*
 * @Author: Naohand 120262455+Naohand@users.noreply.github.com
 * @Date: 2023-04-09 22:03:32
 * @LastEditors: Please set LastEditors
 * @LastEditTime: 2023-10-02 09:47:49
 * @FilePath: McuComm.cpp
 * @Description:
 */
#include <mcu_pack/McuComm.h>

#ifdef TEST_RT_TIME
timeval tstart, tend; // 测试数据传输的时间
#endif
int rx_mcu_flag = 0;
MCU_Comm USB_uart;
TxDate_t TxDate;

char tx_data_pps[20];

union
{
    char dataC[RX_MCU_LENGTH];
    float dataF[RX_MCU_LENGTH / 4];
} mcu_data;

void Embedded_Init()
{
    // 初始化
    std::cout << "embedded init .." << std::endl;
    // 等待MCU初始化
    std::cout << "waiting for usb ..." << std::endl;
    // 配置波特率等参数
    MCU_Comm *comm = new MCU_Comm();
    std::cout << "init done!!!" << std::endl;
}

MCU_Comm::MCU_Comm(void)
{
#ifdef HIDCOM
    commusb = new commUSB;
    commusb->commUSBInit();
#endif

#ifdef UARTCOM
    commserial = new commSerial("/dev/com_port", B115200); // TODO need to change path       Bus 001 Device 008: ID
    commserial->commSerialInit();
    // 0483:5740 STMicroelectronics STM32F407  SUBSYSTEMS=="usb", ATTRS{idVendor}=="0483",
    // ATTRS{idProduct}=="5740",GROUP="users", MODE="0777",SYMLINK+="usb_uart"
    
#endif
}

MCU_Comm::~MCU_Comm(void)
{
#ifdef HIDCOM
    delete commusb;
#endif

#ifdef UARTCOM
    delete commserial;
#endif
}

void MCU_Comm::WaitForMcuPrepared(void)
{
    // std::cout << "start waiting for mcu****************" << std::endl;
    // while (commserial->commSerialRead() != MCU_COMMU_RX_LENGTH)
    // {
    //     std::cout << "waiting for mcu ..." << std::endl;
    // }
}

static uint8_t mcuTalkOk = 0;

void MCU_Comm::Communicate2Mcu(char *data)
{
    clock_t time_start, time_end;
#ifdef TEST_RT_TIME
    gettimeofday(&tstart, NULL);
#endif

#ifdef HIDCOM
    commusb->txbuf[0] = 0x0;
#include <User_usb.h>
    commusb->txbuf[1] = 'H';
    commusb->txbuf[2] = 'M';
    commusb->txbuf[MCU_COMMU_TX_LENGTH - 3] = '\r';
    commusb->txbuf[MCU_COMMU_TX_LENGTH - 2] = '\n';
    commusb->txbuf[MCU_COMMU_TX_LENGTH - 1] = '0';
#endif

#ifdef PRINT_COMMU_TIME
    time_start = clock();
#endif

#ifdef HIDCOM
    commusb->commUSBWrite();
    commusb->commUSBRead();
    commusb->commUSBPrintResult();
#endif

#ifdef UARTCOM
    tx_data_pps[0] = 'A';
    tx_data_pps[1] = 'T';
    tx_data_pps[19] = 't';
    tx_data_pps[20] = 'a';
    for (int i = 0; i < 20; ++i)
    {
        tx_data_pps[i + 2] = data[i];
    }
    commserial->commSerialWrite(tx_data_pps);
    // if (!rx_mcu_flag)
    // {
    // commserial->commSerialRead();
    // // }
    // if (commserial->rxbuf[0] == 'A' && commserial->rxbuf[1] == 'T' && commserial->rxbuf[11] == 't' && commserial->rxbuf[12] == 'a')
    // {
    //     for (int i = 0; i < 8; ++i)
    //     {
    //         mcu_data.dataC[i] = commserial->rxbuf[i + 2];
    //     }
    //     App_target_x = mcu_data.dataF[0];
    //     App_target_y = mcu_data.dataF[1];
    //     rx_mcu_flag = commserial->rxbuf[10];
    // }
    // commserial->commSerialPrintResult();
#endif

#ifdef PRINT_COMMU_TIME
    time_end = clock();
    std::cout << "-----------------------TIME--------------------" << std::endl;
    std::cout << (double)((time_end - time_start) / 1000.0) << std::endl;
#endif

#ifdef HIDCOM
    if (commusb->rxbuf[0] == 'S' && commusb->rxbuf[1] == 'M' && commusb->rxbuf[MCU_COMMU_RX_LENGTH - 3] == '\r' && commusb->rxbuf[MCU_COMMU_RX_LENGTH - 2] == '\n')
    {
        for (int i = 0; i < (MCU_COMMU_RX_LENGTH - 5); i++)
        {
            usb_rx_transdata[i] = commusb->rxbuf[i + 2];
        }
        ReadUSBData(usb_rx_transdata);
    }
    else if (commusb->rxbuf[1] == 'S' && commusb->rxbuf[2] == 'M' && commusb->rxbuf[MCU_COMMU_RX_LENGTH - 2] == '\r' && commusb->rxbuf[MCU_COMMU_RX_LENGTH - 1] == '\n')
    {
        for (int i = 0; i < (MCU_COMMU_RX_LENGTH - 5); i++)
        {
            usb_rx_transdata[i] = commusb->rxbuf[i + 3];
        }
        ReadUSBData(usb_rx_transdata);
    }
#endif

#ifdef USB_TEST
    std::cout << " =========================== rxbuf - char ===========================" << std::endl;
    for (int i = 0; i < (MCU_COMMU_RX_LENGTH); i++)
    {
        std::cout << commusb->rxbuf[i];
    }
    std::cout << std::endl;

    std::cout << " =========================== usb_tx_transdata - char ===========================" << std::endl;
    for (int i = 0; i < (MCU_COMMU_RX_LENGTH - 5); i++)
    {
        std::cout << usb_rx_transdata[i];
    }
    std::cout << std::endl;
#endif

#ifdef UARTCOM

    // cout << "---rxbuf0 = " << (int)commserial->rxbuf[0] << endl;
    // cout << "---rxbuf1 = " << (int)commserial->rxbuf[1] << endl;

    // cout << "---rxbuf2 = " << (int)commserial->rxbuf[MCU_COMMU_RX_LENGTH-6] << endl;
    // cout << "---rxbuf3 = " << (int)commserial->rxbuf[MCU_COMMU_RX_LENGTH-5] << endl;
    // cout << "---rxbuf4 = " << (int)commserial->rxbuf[MCU_COMMU_RX_LENGTH-4] << endl;

    // cout << "---rxbuf5 = " << (int)commserial->rxbuf[MCU_COMMU_RX_LENGTH-3] << endl;
    // cout << "---rxbuf6 = " << (int)commserial->rxbuf[MCU_COMMU_RX_LENGTH-2] << endl;
    // cout << "---rxbuf7 = " << (int)commserial->rxbuf[MCU_COMMU_RX_LENGTH-1] << endl;
    // TODO receive data need recompose
    // if (commserial->rxbuf[0] == 'S' && commserial->rxbuf[1] == 'M' && commserial->rxbuf[MCU_COMMU_RX_LENGTH - 3] == 'M' && commserial->rxbuf[MCU_COMMU_RX_LENGTH - 2] == 'S')
    // {
    //     cout << "-----------" << endl;
    //     for (int i = 0; i < (MCU_COMMU_RX_LENGTH - 5); i++)
    //     {
    //         usb_rx_transdata[i] = commserial->rxbuf[i + 2];
    //     }
    //     //ReadUSBData(usb_rx_transdata);
    // }
    // else if (commserial->rxbuf[1] == 'S' && commserial->rxbuf[2] == 'M' && commserial->rxbuf[MCU_COMMU_RX_LENGTH - 2] == 'M' && commserial->rxbuf[MCU_COMMU_RX_LENGTH - 1] == 'S')
    // {
    //     cout << "-----------" << endl;
    //     for (int i = 0; i < (MCU_COMMU_RX_LENGTH - 5); i++)
    //     {
    //         usb_rx_transdata[i] = commserial->rxbuf[i + 3];
    //     }
    //    // ReadUSBData(usb_rx_transdata);
    // }
#endif

//     for (uint8_t i = 0; i < MCU_COMMU_RX_LENGTH; i++)
//     {
// #ifdef HIDCOM
//         commusb->rxbuf[i] = '0';
// #endif

// #ifdef UARTCOM
//         commserial->rxbuf[i] = '0';
// #endif
//     }
//     for (uint8_t i = 0; i < (MCU_COMMU_RX_LENGTH - 5); i++)
//     {
//         usb_rx_transdata[i] = '0';
//     }
#ifdef TEST_RT_TIME
    gettimeofday(&tend, NULL);
#endif
}
void MCU_Comm::Read2McuData()
{
    commserial->commSerialRead();
    if (commserial->rxbuf[0] == 'A' && commserial->rxbuf[1] == 'T' && commserial->rxbuf[11] == 't' && commserial->rxbuf[12] == 'a')
    {
        for (int i = 0; i < 8; ++i)
        {
            mcu_data.dataC[i] = commserial->rxbuf[i + 2];
        }
    }
}
void MCU_Comm::WriteUSBData(char *data)
{

    union trans_t
    {
        float dataf[11];
        char datac[11 * 4];
    } trans;

    // 赋值float 型数据
    trans.dataf[0] = 13.137;
    trans.dataf[1] = 13.137;
    trans.dataf[2] = 13.137;
    trans.dataf[3] = 13.137;

    trans.dataf[4] = 13.137;
    trans.dataf[5] = 13.137;
    trans.dataf[6] = 13.137;
    trans.dataf[7] = 13.137;

    trans.dataf[8] = 13.137;
    trans.dataf[9] = 13.137;
    trans.dataf[10] = 123.456;

#ifdef USB_TEST
    for (int i = 0; i < 15; i++)
    {
        trans.dataf[i] = 1.2;
    }
    std::cout << " =========================== usb_tx_transdata - float ===========================" << std::endl;
    for (int i = 0; i < 14; i++)
    {
        std::cout << trans.dataf[i];
    }
    std::cout << std::endl;
#endif

    for (int j = 0; j < 11 * 4; j++)
    {
        data[j] = trans.datac[j];
    }
}

// void MCU_Comm::ReadUSBData(char *data)
// {
//     union trans_t // 驱动器信息
//     {
//         float dataf[4];
//         char datac[4 * 4];
//     } trans;

//     for (int j = 0; j < 4 * 4; j++)
//     {
//         trans.datac[j] = data[j];
//     }

//     gRobot.mcuData.startProFlag = data[MCU_COMMU_RX_LENGTH - 6];
//     gRobot.mcuData.lowPowerFlag = data[MCU_COMMU_RX_LENGTH - 7];
//     gRobot.mcuData.lightFlag[0] = data[MCU_COMMU_RX_LENGTH - 8]; // left
//     gRobot.mcuData.lightFlag[1] = data[MCU_COMMU_RX_LENGTH - 9]; // right

//     cout << "------left lightFlag = " << (int)gRobot.mcuData.lightFlag[0] << endl;
//     cout << "------right lightFlag = " << (int)gRobot.mcuData.lightFlag[1] << endl;

//     cout << "----------lowpowerFlag = " << (int)gRobot.mcuData.lowPowerFlag << endl;

// #ifdef USB_TEST
//     std::cout << " =========================== usb_rx_transdata - 2 - float ===========================" << std::endl;
//     for (int i = 0; i < 9; i++)
//     {
//         std::cout << (float)(trans_2.dataf[i] / 1000.0) << "  ";
//     }
//     std::cout << std::endl;
// #endif
// }

/*
    embedded.cpp:   WaitMcuPrepared();
        mcucomm.cpp:    comm->WaitForMcuPrepared();
                            std:: cout << "waiting for mcu****************" << std::endl;
                            while(commSerial.cpp:   commserial->commSerialRead() != MCU_COMMU_RX_LENGTH){
                                std:: cout << "waiting for mcu ..." << std::endl;
                            }
                                int commSerial::commSerialRead(){
                                    int len = 0;
                                    len = read(handle, rxbuf, RX_DATA_LEN);//读数据长度
                                    std::cout << "len:" << len << std::endl;
                                    return len;
                                }
    embedded.cpp:   Embedded();
        mcucomm.cpp:    comm->Communicate2Mcu();

*/